#ifndef TWAI_HELPER
#define TWAI_HELPER

#include "common_config.h"
#include "icreader.h"
#include "driver/twai.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
//CAN
#define EXAMPLE_TAG "TWAI"
#define PING_PERIOD_MS          50
#define NO_OF_DATA_MSGS         50
#define NO_OF_ITERS             3
#define ITER_DELAY_MS           1000
#define RX_TASK_PRIO            1
#define TX_TASK_PRIO            5
#define CTRL_TSK_PRIO           10
#define CAN_TX_GPIO_NUM         33
#define CAN_RX_GPIO_NUM         32

#define ID_MASTER_STOP_CMD      0x0A0
#define ID_MASTER_START_CMD     0x0A1
#define ID_MASTER_PING          0x0A2
#define ID_SLAVE_STOP_RESP      0x0B0
#define ID_SLAVE_DATA           0x0B1
#define ID_SLAVE_PING_RESP      0x0B2

#define ID_REQ_CMD 0x000007DF
#define ID_REC_CMD 0x000007E8


typedef enum {
    TX_SEND_PINGS,
    TX_SEND_START_CMD,
    TX_SEND_STOP_CMD,
    TX_TASK_EXIT,
} tx_task_action_t;

typedef enum {
    RX_RECEIVE_PING_RESP,
    RX_RECEIVE_DATA,
    RX_RECEIVE_STOP_RESP,
    RX_TASK_EXIT,
} rx_task_action_t;

static const twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();
// static const twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
static const twai_filter_config_t f_config = {
    .acceptance_code = 0x000007E8,
    .acceptance_mask = 0xffffffff,
    .single_filter = true
};
static const twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(CAN_TX_GPIO_NUM, CAN_RX_GPIO_NUM, TWAI_MODE_NORMAL);

static const twai_message_t ping_message = {.identifier = ID_REQ_CMD, 
.data_length_code = 8,

                                           .ss = 1,
                                            .data = {0x02, 0x01 , 0x0C, 0 ,0 ,0 ,0 ,0}};
// static const twai_message_t start_message = {.identifier = ID_MASTER_START_CMD, .data_length_code = 0,
//                                             .data = {0, 0 , 0 , 0 ,0 ,0 ,0 ,0}};
// static const twai_message_t stop_message = {.identifier = ID_MASTER_STOP_CMD, .data_length_code = 0,
//                                            .data = {0, 0 , 0 , 0 ,0 ,0 ,0 ,0}};

static QueueHandle_t tx_task_queue;
static QueueHandle_t rx_task_queue;
static SemaphoreHandle_t stop_ping_sem;
static SemaphoreHandle_t ctrl_task_sem;
static SemaphoreHandle_t done_sem;

static void twai_receive_task(void *arg)
{
    while (1) {
        char *RPM;
        twai_message_t rx_msg;
        if (twai_receive(&rx_msg, portMAX_DELAY) == ESP_OK)
        {
            // ESP_LOGI(EXAMPLE_TAG, "Identifier: %u", (char)rx_msg.identifier);
            if (rx_msg.identifier == ID_REC_CMD) {
                int rpm;
                RPM = (char*)&rpm;
                RPM[0] = rx_msg.data[4];
                RPM[1] = rx_msg.data[3];
                ESP_LOGI(EXAMPLE_TAG, "RPM value %d", rpm/4);
                if (rpm != 0)
                {
                    uart_write_bytes(UART_4G_NUM, (const char*) engine_on, sizeof(engine_on));
                }
                else{
                    uart_write_bytes(UART_4G_NUM, (const char*) engine_off, sizeof(engine_off));
            }
        }
        }
        
        
        vTaskDelay(pdMS_TO_TICKS(10));

        // rx_task_action_t action;
        // xQueueReceive(rx_task_queue, &action, portMAX_DELAY);

        // if (action == RX_RECEIVE_PING_RESP) {
        //     //Listen for ping response from slave
        //     while (1) {
        //         twai_message_t rx_msg;
        //         twai_receive(&rx_msg, portMAX_DELAY);
        //         if (rx_msg.identifier == ID_SLAVE_PING_RESP) {
        //             xSemaphoreGive(stop_ping_sem);
        //             xSemaphoreGive(ctrl_task_sem);
        //             break;
        //         }
        //     }
        // } else if (action == RX_RECEIVE_DATA) {
        //     //Receive data messages from slave
        //     uint32_t data_msgs_rec = 0;
        //     // while (data_msgs_rec < NO_OF_DATA_MSGS) {
        //     while (1) {
        //         twai_message_t rx_msg;
        //         twai_receive(&rx_msg, portMAX_DELAY);
        //         if (rx_msg.identifier == ID_SLAVE_DATA) {
        //             uint32_t data = 0;
        //             for (int i = 0; i < rx_msg.data_length_code; i++) {
        //                 data |= (rx_msg.data[i] << (i * 8));
        //             }
        //             ESP_LOGI(EXAMPLE_TAG, "Received data value %"PRIu32, data);
        //             data_msgs_rec ++;
        //         }
        //     }
        //     xSemaphoreGive(ctrl_task_sem);
        // } else if (action == RX_RECEIVE_STOP_RESP) {
        //     //Listen for stop response from slave
        //     while (1) {
        //         twai_message_t rx_msg;
        //         twai_receive(&rx_msg, portMAX_DELAY);
        //         if (rx_msg.identifier == ID_SLAVE_STOP_RESP) {
        //             xSemaphoreGive(ctrl_task_sem);
        //             break;
        //         }
        //     }
        // } else if (action == RX_TASK_EXIT) {
        //     break;
        // }



    }
    vTaskDelete(NULL);
}

static void twai_transmit_task(void *arg)
{
    while (1) {
        twai_transmit(&ping_message, portMAX_DELAY);
        // ESP_LOGI("CAN: ", "Transmited.");
        vTaskDelay(pdMS_TO_TICKS(PING_PERIOD_MS));


        // tx_task_action_t action;
        // xQueueReceive(tx_task_queue, &action, portMAX_DELAY);
        // if (action == TX_SEND_PINGS) {
        //     //Repeatedly transmit pings
        //     ESP_LOGI(EXAMPLE_TAG, "Transmitting ping");
        //     while (xSemaphoreTake(stop_ping_sem, 0) != pdTRUE) {
        //         twai_transmit(&ping_message, portM   AX_DELAY);
        //         vTaskDelay(pdMS_TO_TICKS(PING_PERIOD_MS));
        //     }
        // } else if (action == TX_SEND_START_CMD) {
        //     //Transmit start command to slave
        //     twai_transmit(&start_message, portMAX_DELAY);
        //     ESP_LOGI(EXAMPLE_TAG, "Transmitted start command");
        // } else if (action == TX_SEND_STOP_CMD) {
        //     //Transmit stop command to slave
        //     twai_transmit(&stop_message, portMAX_DELAY);
        //     ESP_LOGI(EXAMPLE_TAG, "Transmitted stop command");
        // } else if (action == TX_TASK_EXIT) {
        //     break;
        // }


    }
    vTaskDelete(NULL);
}

static void twai_control_task(void *arg)
{
    xSemaphoreTake(ctrl_task_sem, portMAX_DELAY);
    tx_task_action_t tx_action;
    rx_task_action_t rx_action;

    for (int iter = 0; iter < NO_OF_ITERS; iter++) {
        ESP_ERROR_CHECK(twai_start());
        ESP_LOGI(EXAMPLE_TAG, "Driver started");

        //Start transmitting pings, and listen for ping response
        tx_action = TX_SEND_PINGS;
        rx_action = RX_RECEIVE_PING_RESP;
        xQueueSend(tx_task_queue, &tx_action, portMAX_DELAY);
        xQueueSend(rx_task_queue, &rx_action, portMAX_DELAY);

        //Send Start command to slave, and receive data messages
        xSemaphoreTake(ctrl_task_sem, portMAX_DELAY);
        tx_action = TX_SEND_START_CMD;
        rx_action = RX_RECEIVE_DATA;
        xQueueSend(tx_task_queue, &tx_action, portMAX_DELAY);
        xQueueSend(rx_task_queue, &rx_action, portMAX_DELAY);

        //Send Stop command to slave when enough data messages have been received. Wait for stop response
        xSemaphoreTake(ctrl_task_sem, portMAX_DELAY);
        tx_action = TX_SEND_STOP_CMD;
        rx_action = RX_RECEIVE_STOP_RESP;
        xQueueSend(tx_task_queue, &tx_action, portMAX_DELAY);
        xQueueSend(rx_task_queue, &rx_action, portMAX_DELAY);

        xSemaphoreTake(ctrl_task_sem, portMAX_DELAY);
        ESP_ERROR_CHECK(twai_stop());
        ESP_LOGI(EXAMPLE_TAG, "Driver stopped");
        vTaskDelay(pdMS_TO_TICKS(ITER_DELAY_MS));
    }
    //Stop TX and RX tasks
    tx_action = TX_TASK_EXIT;
    rx_action = RX_TASK_EXIT;
    xQueueSend(tx_task_queue, &tx_action, portMAX_DELAY);
    xQueueSend(rx_task_queue, &rx_action, portMAX_DELAY);

    //Delete Control task
    xSemaphoreGive(done_sem);
    vTaskDelete(NULL);
}

#endif